#include "gnss_parser.h"

ErrorCode gnss_parse_nmea(const char* nmea_sentence, GNSS_Satellite* satellites, int* count) {
    if (!nmea_sentence || !satellites || !count) return ERROR_INVALID_INPUT;
    
    // 简化的NMEA解析，实际实现会更复杂
    *count = 0;
    
    // 模拟解析GPGGA语句
    if (strncmp(nmea_sentence, "$GPGGA", 6) == 0) {
        // 这里应该解析实际的NMEA数据
        // 为了演示，我们创建一些模拟数据
        satellites[0].prn = 1;
        satellites[0].position = vector3d_create(26560000.0, 0.0, 0.0);
        satellites[0].velocity = vector3d_create(0.0, 3874.0, 0.0);
        satellites[0].clock_bias = 0.0;
        satellites[0].signal_strength = 45.0;
        satellites[0].pseudorange = 20000000.0;
        satellites[0].doppler_shift = 0.0;
        
        *count = 1;
    }
    
    return SUCCESS;
}

ErrorCode gnss_calculate_position(GNSS_Satellite* satellites, int count, GNSS_Receiver* receiver) {
    if (!satellites || !receiver || count < 4) return ERROR_INVALID_INPUT;
    
    // 简化的位置计算，使用最小二乘法
    // 实际实现会使用卡尔曼滤波
    
    // 模拟位置计算
    receiver->position = vector3d_create(7000000.0, 0.0, 0.0);  // LEO轨道
    receiver->velocity = vector3d_create(0.0, 7500.0, 0.0);    // 轨道速度
    receiver->clock_bias = 0.001;
    receiver->clock_drift = 0.000001;
    receiver->visible_satellites = count;
    receiver->pdop = 2.5;
    receiver->hdop = 1.8;
    receiver->vdop = 1.2;
    
    return SUCCESS;
}

ErrorCode gnss_simulate_data(GNSS_Satellite* satellites, int* count, GNSS_Receiver* receiver) {
    if (!satellites || !count || !receiver) return ERROR_INVALID_INPUT;
    
    static double time = 0.0;
    time += 1.0;
    
    // 模拟8颗GPS卫星
    *count = 8;
    
    for (int i = 0; i < *count; i++) {
        satellites[i].prn = i + 1;
        
        // 卫星轨道参数
        double a = 26560000.0;  // 轨道半长轴
        double e = 0.0;         // 偏心率
        double i = 55.0 * DEG_TO_RAD;  // 倾角
        double omega = 0.0;    // 近地点幅角
        double omega_dot = 0.0;  // 升交点赤经变化率
        
        // 计算卫星位置
        double theta = (2.0 * PI * i / 8.0) + (time * 0.0001);
        satellites[i].position.x = a * cos(theta);
        satellites[i].position.y = a * sin(theta) * cos(i);
        satellites[i].position.z = a * sin(theta) * sin(i);
        
        // 计算卫星速度
        double v = sqrt(MU / a);
        satellites[i].velocity.x = -v * sin(theta);
        satellites[i].velocity.y = v * cos(theta) * cos(i);
        satellites[i].velocity.z = v * cos(theta) * sin(i);
        
        satellites[i].clock_bias = 0.0;
        satellites[i].signal_strength = 40.0 + 10.0 * sin(time * 0.01);
        satellites[i].pseudorange = gnss_calculate_distance(receiver->position, satellites[i].position);
        satellites[i].doppler_shift = 0.0;
    }
    
    // 更新接收机位置
    receiver->position.x = 7000000.0 * cos(time * 0.001);
    receiver->position.y = 7000000.0 * sin(time * 0.001);
    receiver->position.z = 0.0;
    
    receiver->velocity.x = -7500.0 * sin(time * 0.001);
    receiver->velocity.y = 7500.0 * cos(time * 0.001);
    receiver->velocity.z = 0.0;
    
    return SUCCESS;
}

Vector3D gnss_ecef_to_geodetic(Vector3D ecef) {
    // WGS84参数
    const double a = 6378137.0;
    const double e2 = 6.69437999014e-3;
    
    double x = ecef.x;
    double y = ecef.y;
    double z = ecef.z;
    
    double p = sqrt(x * x + y * y);
    double theta = atan2(z * a, p * sqrt(1.0 - e2));
    
    double lat = atan2(z + e2 * a * pow(sin(theta), 3) / sqrt(1.0 - e2),
                     p - e2 * a * pow(cos(theta), 3));
    double lon = atan2(y, x);
    double alt = p / cos(lat) - a / sqrt(1.0 - e2 * sin(lat) * sin(lat));
    
    return vector3d_create(lat * RAD_TO_DEG, lon * RAD_TO_DEG, alt);
}

Vector3D gnss_geodetic_to_ecef(Vector3D geodetic) {
    // WGS84参数
    const double a = 6378137.0;
    const double e2 = 6.69437999014e-3;
    
    double lat = geodetic.x * DEG_TO_RAD;
    double lon = geodetic.y * DEG_TO_RAD;
    double alt = geodetic.z;
    
    double N = a / sqrt(1.0 - e2 * sin(lat) * sin(lat));
    
    double x = (N + alt) * cos(lat) * cos(lon);
    double y = (N + alt) * cos(lat) * sin(lon);
    double z = (N * (1.0 - e2) + alt) * sin(lat);
    
    return vector3d_create(x, y, z);
}

double gnss_calculate_distance(Vector3D a, Vector3D b) {
    return vector3d_distance(a, b);
}

double gnss_calculate_pseudorange(Vector3D receiver, Vector3D satellite, double clock_bias) {
    double geometric_distance = vector3d_distance(receiver, satellite);
    return geometric_distance + clock_bias * 299792458.0;  // 光速
}

void gnss_print_receiver(const GNSS_Receiver* receiver) {
    if (!receiver) return;
    
    printf("=== GNSS接收机状态 ===\n");
    printf("位置 (ECEF): ");
    vector3d_print("", receiver->position);
    
    Vector3D geodetic = gnss_ecef_to_geodetic(receiver->position);
    printf("位置 (LLA): [%.6f°, %.6f°, %.1f m]\n", 
           geodetic.x, geodetic.y, geodetic.z);
    
    printf("速度: ");
    vector3d_print("", receiver->velocity);
    
    printf("时钟偏差: %.6f s\n", receiver->clock_bias);
    printf("可见卫星: %d\n", receiver->visible_satellites);
    printf("PDOP: %.2f\n", receiver->pdop);
    printf("HDOP: %.2f\n", receiver->hdop);
    printf("VDOP: %.2f\n", receiver->vdop);
}
